#include <stdio.h>
#include "crc.h"

using namespace std;

static int              crc_tabccitt_init       = FALSE;

static unsigned short   crc_tabccitt[256];


unsigned short update_crc_ccitt( uint16_t crc, uint8_t c ) {

    unsigned short tmp, short_c;

    short_c  = 0x00ff & (unsigned short) c;

    if ( ! crc_tabccitt_init ) init_crcccitt_tab();

    tmp = (crc >> 8) ^ short_c;
    crc = (crc << 8) ^ crc_tabccitt[tmp];

    return crc;

}  /* update_crc_ccitt */

void init_crcccitt_tab( void ) {

    int i, j;
    unsigned short crc, c;

    for (i=0; i<256; i++) {

        crc = 0;
        c   = ((unsigned short) i) << 8;

        for (j=0; j<8; j++) {

            if ( (crc ^ c) & 0x8000 ) crc = ( crc << 1 ) ^ P_CCITT;
            else                      crc =   crc << 1;

            c = c << 1;
        }

        crc_tabccitt[i] = crc;
    }

    crc_tabccitt_init = TRUE;

}  /* init_crcccitt_tab */



void crcAppend(uint8_t *packet,uint16_t packetLength)
{
  uint16_t i;

  WORD_t crc;

  crc.word = 0xFFFF;

  for(i=0;i<packetLength-2;i++)
    {
      crc.word = update_crc_ccitt(crc.word,packet[i]);
    }

  packet[packetLength-2] = crc.bytes.byteH;
  packet[packetLength-1] = crc.bytes.byteL;
}

uint8_t crcCheck(uint8_t *packet,uint16_t packetLength)
{
  uint16_t i;

  WORD_t crc;

  crc.word = 0xFFFF;

  for(i=0;i<packetLength-2;i++)
  {
    crc.word = update_crc_ccitt(crc.word,packet[i]);
  }

//  printf("CrcCheck: %x-%x vs %x-%x \n", packet[packetLength-2],packet[packetLength-1],crc.bytes.byteH,crc.bytes.byteL );
  if( (packet[packetLength-2] == crc.bytes.byteH) && (packet[packetLength-1] = crc.bytes.byteL) )
  {
    return 1;
  }
  else
  {
    return 0;
  }
}
